#ifndef CONTROL_H_
#define CONTROL_H_

#include <cmath>
#include <iostream>
#include "interface/control_interface.h"
#include "third_party/include/simple_time.h"
#include "can/can.h"

/**
 * @brief 控制类，用于处理车辆的控制命令
 *
 * 该类继承自ControllerInterface接口，实现了对车辆的控制逻辑。
 * 它通过CAN总线与车辆的各个部件通信，控制车辆的行驶和转向。
 */
class Control : public ControllerInterface {
public:
    /**
     * @brief 构造函数
     *
     * @param name 控制器的名称，用于CAN总线的初始化
     */
    Control(const char *name)
        : can_(std::make_unique<Can>(name))
        , stop_(false)
        , wheelbase_(0.877)
        , wheel_radius_(0.115)
        , driver_motor_reduction_rate_(28.0)
        , steer_motor_reduction_rate_(172.6)
        , max_liner_velocity_(1.5)
        , max_steer_angle_(90 * M_PI / 180)
        , max_angle_velocity_(max_liner_velocity_ * sin(max_steer_angle_) / wheelbase_) {
        odom_ = Odometry{0, 0, 0};
        std::cout << "wheelbase_:" << wheelbase_ << std::endl;
        std::cout << "wheel_radius_:" << wheel_radius_ << std::endl;
        std::cout << "driver_motor_reduction_rate_:" << driver_motor_reduction_rate_ << std::endl;
        std::cout << "steer_motor_reduction_rate_:" << steer_motor_reduction_rate_ << std::endl;
        std::cout << "max_steer_angle_:" << max_steer_angle_ << std::endl;
        std::cout << "max_liner_velocity_:" << max_liner_velocity_ << std::endl;
        std::cout << "max_angle_velocity_:" << max_angle_velocity_ << std::endl;

        // 开启接收命令的线程
        std::thread handle_cmd_thread(&Control::ReceiveCmdHandleThread, this);
        handle_cmd_thread.detach();
    }

    // 启动接收命令处理线程
    // 该线程负责持续监听和处理接收到的CAN命令
    void ReceiveCmdHandleThread();

    // 解析接收到的CAN帧
    // 参数frame是接收到的CAN帧，包含帧ID和数据等信息
    // 该函数根据帧ID和数据内容执行相应的处理逻辑
    void ParseFrame(const struct can_frame &frame);

    // 发送速度命令
    void sendVelocity(float linear_velocity[3], float angular_velocity[3]) override;

    std::pair<uint64_t, std::vector<float>> getOdometry() override;

    // 将线速度转换为每分钟转速(RPM)
    int16_t LinearVelocitytoRPM(float linear_velocity);

    float RPM2LinearVelocity(int16_t wheel_rpm);

    // 限制线速度和角速度
    void LimitVelocity(float *linear_velocity, float *angular_velocity);

private:
    struct Odometry {
        uint64_t time;
        float    x;
        float    y;
        float    yaw;
    };

    std::unique_ptr<Can> can_;

    bool stop_;  // 控制线程是否停止

    float wheelbase_;                    // 轴距
    float wheel_radius_;                 // 轮子半径
    float driver_motor_reduction_rate_;  // 行驶电机减速比
    float steer_motor_reduction_rate_;   // 转向电机减速比

    const float max_liner_velocity_;  // 最大线速度
    const float max_steer_angle_;     // 最大转向角度
    const float max_angle_velocity_;  // 最大转向角速度

    Odometry   odom_;   // 里程计位姿
};

#endif